#include "fm33le0xx_fl.h"
#include "sys_hardware.h"

#define PLL_TIMEOUT 0xFFFFFFFFU
uint32_t g_ticks_ms = 0;

void RCC_PLL_ConfigDomain_SYS(uint32_t Source, uint32_t PLL_R, uint32_t PLL_DB, uint32_t PLL_O) {
    MODIFY_REG(RCC->PLLCR, RCC_PLLCR_DB_Msk | RCC_PLLCR_REFPRSC_Msk | RCC_PLLCR_OSEL_Msk | RCC_PLLCR_INSEL_Msk,
               ((PLL_DB - 1) << RCC_PLLCR_DB_Pos) | PLL_R | PLL_O | Source);
}

void sys_select_rchf_to_pll(uint8_t freq) {
    FL_RCC_RCHF_WriteTrimValue(RCHF8M_TRIM);
    FL_RCC_RCHF_SetFrequency(FL_RCC_RCHF_FREQUENCY_8MHZ);
    FL_RCC_RCHF_Enable();
    FL_FLASH_SetReadWait(FLASH, FL_FLASH_READ_WAIT_2CYCLE);
    RCC_PLL_ConfigDomain_SYS(FL_RCC_PLL_CLK_SOURCE_RCHF, FL_RCC_PLL_PSC_DIV8, freq, FL_RCC_PLL_OUTPUT_X1);
    FL_RCC_PLL_Enable(); // 使能PLL
    while(FL_RCC_IsActiveFlag_PLLReady() != FL_SET); // 等待PLL建立稳定，才能将系统时钟源切到PLL

    FL_RCC_SetAHBPrescaler(FL_RCC_AHBCLK_PSC_DIV1);
    FL_RCC_SetAPB1Prescaler(FL_RCC_APB1CLK_PSC_DIV1);
    FL_RCC_SetAPB2Prescaler(FL_RCC_APB2CLK_PSC_DIV1);
    FL_RCC_SetSystemClockSource(FL_RCC_SYSTEM_CLK_SOURCE_PLL);

    if (freq <= 24) {
        FL_FLASH_SetReadWait(FLASH, FL_FLASH_READ_WAIT_0CYCLE);
    } else if ((freq > 24) && (freq <= 48)) {
        FL_FLASH_SetReadWait(FLASH, FL_FLASH_READ_WAIT_1CYCLE);
    } else {
        FL_FLASH_SetReadWait(FLASH, FL_FLASH_READ_WAIT_2CYCLE);
    }

    SystemCoreClockUpdate();
}

void sys_select_lp_clk(void) {
    FL_RCC_SetLSCLKClockSource(FL_RCC_LSCLK_CLK_SOURCE_LPOSC);
    FL_RCC_SetSystemClockSource(FL_RCC_SYSTEM_CLK_SOURCE_LSCLK); 
    FL_FLASH_SetReadWait(FLASH, FL_FLASH_READ_WAIT_1CYCLE);
    FL_RCC_SetAHBPrescaler(FL_RCC_AHBCLK_PSC_DIV1);
    FL_RCC_SetAPB1Prescaler(FL_RCC_APB1CLK_PSC_DIV1);
    FL_RCC_SetAPB2Prescaler(FL_RCC_APB2CLK_PSC_DIV1);
    SystemCoreClockUpdate();
}

void sys_select_rchf_8m_clk(void) {
    FL_RCC_RCHF_SetFrequency(FL_RCC_RCHF_FREQUENCY_8MHZ);
    FL_RCC_RCHF_Enable();
    FL_RCC_SetSystemClockSource(FL_RCC_SYSTEM_CLK_SOURCE_RCHF); 
    FL_FLASH_SetReadWait(FLASH, FL_FLASH_READ_WAIT_0CYCLE);
    FL_RCC_SetAHBPrescaler(FL_RCC_AHBCLK_PSC_DIV1);
    FL_RCC_SetAPB1Prescaler(FL_RCC_APB1CLK_PSC_DIV1);
    FL_RCC_SetAPB2Prescaler(FL_RCC_APB2CLK_PSC_DIV1);
    SystemCoreClockUpdate();
}

void sys_clock_init(void) {
    FL_LPTIM32_InitTypeDef lptim32;
    lptim32.clockSource = FL_RCC_LPTIM32_CLK_SOURCE_LPOSC;
    lptim32.prescalerClockSource = FL_LPTIM32_CLK_SOURCE_INTERNAL;
    lptim32.prescaler = FL_LPTIM32_PSC_DIV32;
    lptim32.autoReload = 0xffffffff;
    lptim32.mode = FL_LPTIM32_OPERATION_MODE_NORMAL;
    lptim32.onePulseMode = FL_LPTIM32_ONE_PULSE_MODE_CONTINUOUS;
    lptim32.countEdge = FL_LPTIM32_ETR_COUNT_EDGE_RISING;
    lptim32.triggerEdge = FL_LPTIM32_ETR_TRIGGER_EDGE_RISING;
    FL_LPTIM32_Init(LPTIM32, &lptim32);
    FL_LPTIM32_Enable(LPTIM32);
}

uint32_t sys_clock_get_ms(void) {
    static uint32_t last_ms = 0;
    uint32_t now_value = LPTIM32->CNT;
    if (now_value > last_ms) {
        last_ms = now_value;  
    }
    return last_ms;
}

void sys_clock_delay_ms(uint32_t ms) {
    uint32_t ticks_start = sys_clock_get_ms();
    while(sys_clock_get_ms() - ticks_start < ms);
}
